#include "I2Cdev.h"
#include "MPU6050.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;

bool blinkState = false;
int xc, yc;
int a=0;

void setup() {
  // koppel de I2C bus (I2Cdev library doesn't do this automatically)
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif
  // Start de seriele poort.
  Serial.begin(9600);
  // Module intialiseren (i2c)
  accelgyro.initialize();
}

void loop() {
  // lees de waarden accel/gyro uit de module.
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    
  // Deze IF loop slaat de allereerste gemeten waarde van de coordinaten op,
  // om zo een correctie uit te voeren en in horizontale stand op 0 te beginnen.
  if (a < 1) {
    xc = ax;
    yc = ay;
    a = 1;
  } else {
  int x = ax;
  int y = ay;

  // MAP de getallen van X en Y MAX en MIN van 0 t/m 1000.
  float xval = map(x - xc, -16000, 16000, 0, 1000);
  float yval = map(y - yc, -16000, 16000, 0, 1000);  
  // Deze correctie is nodig omdat de gegevens soms net
  // iets buiten het bereik kunnen komen.
  if (xval > 1000) {xval = 1000;}
  if (xval < 0) {xval = 0;}
  if (yval > 1000) {yval = 1000;}
  if (yval < 0) {yval = 0;}
  // Print de gegevens X en Y met als scheidingsteken een komma.
  // Ps. vergeet niet af te sluiten met een nieuwe lijn.
  Serial.print(xval / 1000);
  Serial.print(",");
  Serial.println(yval / 1000);
  // Lees de module uit elke 0,05 seconden.
  delay(50);
  }
}